#include "SimpleFluidCoupler.h"
#include "DecoArticulatedBody.h"
#include "DecoLogger.h"

SimpleFluidCoupler::SimpleFluidCoupler() : mBody(NULL)
{

}
SimpleFluidCoupler::SimpleFluidCoupler(DecoArticulatedObject* obj) : mBody(obj)
{

}
SimpleFluidCoupler::~SimpleFluidCoupler()
{

}

void SimpleFluidCoupler::SetArticulatedObject(DecoArticulatedObject* obj)
{
	mBody = obj;
}
Vecd SimpleFluidCoupler::GetExternalTorque()
{
	const double rho = 1000.0;
	int numLinks = mBody->GetNumBodies();
	int numDofs = mBody->GetNumDofs();
	Model* model = mBody->GetModel();
	Vecd ret(numDofs,vl_0);
	for (int i = 0; i < numLinks; ++i)
	{
		BodyLink* b = model->getBodyLink(i);

		Mat4d transformVL = b->getWorldTransform();
		matrix44 transform = FromMat4dToMatrix44(transformVL);
		matrix44 invTransform = transform.invert();

		DecoRenderData* rData = mBody->GetRenderData(i);
		DecoStaticMesh* linkMesh = static_cast<DecoStaticMesh*>(rData);
		std::vector<vector3> vertices;
		std::vector<vector3> normals;
		linkMesh->RetrieveAllVertices(vertices);
		int numFaces = static_cast<int>(vertices.size()) / 3;

		for (int ithFace = 0; ithFace < numFaces; ++ithFace)
		{
			vector3 v1 = vertices[ithFace * 3];
			vector3 v2 = vertices[ithFace * 3 + 1];
			vector3 v3 = vertices[ithFace * 3 + 2];

			vector3 edge1 = v2 - v1;
			vector3 edge2 = v3 - v1;
			vector3 faceNormal = CrossProduct(edge1, edge2);
			faceNormal.normalize();
			double faceArea = 0.5 * CrossProduct(edge1, edge2).length();
			vector3 faceCenterPoint = (v1 + v2 + v3) / 3.0;
			vector3 faceCenterPointLocal = TransformPoint(invTransform, faceCenterPoint);
			Matd jacobian = mBody->calculateJacobian(b, faceCenterPointLocal);
			Vecd qdot = model->getQDot();
			Vecd vel = jacobian * qdot;
			vector3 faceVelocity = -vector3(vel[0], vel[1], vel[2]);
			//faceVelocity = TransformVector(transform, faceVelocity);

			double relVel = DotProduct(faceVelocity, faceNormal);
			int sgn = 1;
			if (relVel < 0)
				sgn = -1;

			vector3 stress = sgn * faceArea * rho * relVel * relVel * faceNormal;
			Vecd generalTorque = mBody->convertForceFromCartesianToGeneral(b, faceCenterPointLocal, stress);
			ret += generalTorque;

		}
	}
	//for (int i = 0; i < numDofs; ++i)
	//	ret[i] = 0.0;
	(*DecoLogger::GetSingleton()) << "The generalized force is :" << ret[0] << ret[1]<< ret[2]<< ret[3]<< ret[4]<< ret[5] << "\n";
	return ret;
}
